/*
* Author: Martin Klein
* Description: The ROBOTFINDER class is a wrapper class which gives the user a 
	utility to find robots that are visible to the camera it provides all
	of the functions the user might need to interface with the camera and
	find other robots.
*/


#ifndef ROBOTFINDER_HPP
#define ROBOTFINDER_HPP

#include "camera.h"
#include "ColorThreshold.h"
#include "Robot.h"
#include <vector>
#include "opencv2/opencv.hpp"
#include "opencv2/core/core.hpp"
#include "opencv/cv.h"
#include "graphics.h"
#include "camera.h"

using namespace std;
using namespace cv;
using namespace SwarmGraphics;

class CameraException: public std::exception{
public:
    CameraException(const char * message): message_(message){}
    ~CameraException() throw(){ }
    const char* what()const throw()  { return message_.c_str(); }
private:
    std::string message_;
};

class ImageProcessingException: public std::exception{
public:
    ImageProcessingException(const char * message): message_(message){}
    ~ImageProcessingException() throw(){ }
    const char* what()const throw()  { return message_.c_str(); }
private:
    std::string message_;
};

/// Handles all of the image processing for the SwarmAlgorithm
class Finder{

public:
	// Constructors	
	Finder();

	// Destructor
	~Finder();	
  
	/// Use to set color thresholds
	void setRobotColorThresholds(vector<ColorThreshold>& thresholds);
	

	
	// Variables
	// Parse image color position variables
	static const int BLUE;
	static const int RED;
	static const int GREEN;
	static const int ROWS;
    	static const int COLS;
	
    // Getters
    /// Get the image that was just processed by getRobotPositions
    Mat getCurrentImage();
    /// Get the binary image that was just processed by getRobotPositions
    vector<Mat> getBinaryImages();
    
    /*! \return all robots which are visible
    * \brief Calling this function performs all of the image processing and 
    * finds the robots
    */
    vector<Robot> getRobotPositions();

 
    vector<ColorThreshold> getRobotColorThresholds();
	

private:
	// Filtering functions
  	void setThresholdsToDefaults();
  	void filter(Mat &image);
	void findCentroids(vector<Point> & contours, double& x_bar, double& y_bar);
	

	// Variables
	ColorThreshold * thresholds_;
	int threshSize_;
	CCamera * camera_;
    
  
    GfxTexture * currentTex_;
    
    vector<GfxTexture *> binImages_;

    GfxTexture * tempTex0_;
    GfxTexture * tempTex1_;
};	






#endif //ROBOTFINDER_HPP
